#include <math.h>
#include <string.h>
#include "Emm_V5.h"
#include "board.h"



/**********************************************************
***	Emm_V5.0步进闭环控制例程
***	编写作者：ZHANGDATOU
***	技术支持：张大头闭环伺服
***	淘宝店铺：https://zhangdatou.taobao.com
***	CSDN博客：http s://blog.csdn.net/zhangdatou666
***	qq交流群：262438510
**********************************************************/

steeping_motor motor[4];

float pos = 0.0f, Motor_Cur_Pos = 0.0f;
float vel = 0.0f, Motor_Vel = 0.0f;
float Emm_L_pos=0.0f,Emm_R_pos=0.0f;

void uart2_sendChar(uint8_t dat) {
  // 当串口0忙的时候等待，不忙的时候再发送传进来的字符
  while (DL_UART_isBusy(UART_Emm_INST) == true)
    ;

  // 发送单个字符
  DL_UART_Main_transmitData(UART_Emm_INST, dat);
}
void usart_SendCmd(uint8_t *cmd, uint8_t len)
{
   // 当前字符串地址不在结尾 并且 字符串首地址不为空
  for (int i=0 ;i<len;i++) {
    uart2_sendChar(cmd[i]);
  }
}


/**
  * @brief    将当前位置清零
  * @param    addr  ：电机地址
  * @retval   地址 + 功能码 + 命令状态 + 校验字节
  */
void Emm_V5_Reset_CurPos_To_Zero(uint8_t addr)
{
    uint8_t cmd[16] = {0};

    // 装载命令
    cmd[0] =  addr;                       // 地址
    cmd[1] =  0x0A;                       // 功能码
    cmd[2] =  0x6D;                       // 辅助码
    cmd[3] =  0x6B;                       // 校验字节

    // 发送命令
    usart_SendCmd(cmd, 4);
    delay_ms(5);
}

/**
  * @brief    解除堵转保护
  * @param    addr  ：电机地址
  * @retval   地址 + 功能码 + 命令状态 + 校验字节
  */
void Emm_V5_Reset_Clog_Pro(uint8_t addr)
{
    uint8_t cmd[16] = {0};

    // 装载命令
    cmd[0] =  addr;                       // 地址
    cmd[1] =  0x0E;                       // 功能码
    cmd[2] =  0x52;                       // 辅助码
    cmd[3] =  0x6B;                       // 校验字节

    // 发送命令
    usart_SendCmd(cmd, 4);
}

/**
  * @brief    读取系统参数
  * @param    addr  ：电机地址
  * @param    s     ：系统参数类型
  * @retval   地址 + 功能码 + 命令状态 + 校验字节
  */
void Emm_V5_Read_Sys_Params(uint8_t addr, SysParams_t s)
{
    uint8_t i = 0;
    uint8_t cmd[16] = {0};

    // 装载命令
    cmd[i] = addr; ++i;                   // 地址

    switch(s)                             // 功能码
    {
        case S_VER  : cmd[i] = 0x1F; ++i; break;
        case S_RL   : cmd[i] = 0x20; ++i; break;
        case S_PID  : cmd[i] = 0x21; ++i; break;
        case S_VBUS : cmd[i] = 0x24; ++i; break;
        case S_CPHA : cmd[i] = 0x27; ++i; break;
        case S_ENCL : cmd[i] = 0x31; ++i; break;
        case S_TPOS : cmd[i] = 0x33; ++i; break;
        case S_VEL  : cmd[i] = 0x35; ++i; break;
        case S_CPOS : cmd[i] = 0x36; ++i; break;
        case S_PERR : cmd[i] = 0x37; ++i; break;
        case S_FLAG : cmd[i] = 0x3A; ++i; break;
        case S_ORG  : cmd[i] = 0x3B; ++i; break;
        case S_Conf : cmd[i] = 0x42; ++i; cmd[i] = 0x6C; ++i; break;
        case S_State: cmd[i] = 0x43; ++i; cmd[i] = 0x7A; ++i; break;
        default: break;
    }

    cmd[i] = 0x6B; ++i;                   // 校验字节

    // 发送命令
    usart_SendCmd(cmd, i);
    delay_ms(5);

}

/**
  * @brief    修改开环/闭环控制模式
  * @param    addr     ：电机地址
  * @param    svF      ：是否存储标志，false为不存储，true为存储
  * @param    ctrl_mode：控制模式（对应屏幕上的P_Pul菜单），0是关闭脉冲输入引脚，1是开环模式，2是闭环模式，3是让En端口复用为多圈限位开关输入引脚，Dir端口复用为到位输出高电平功能
  * @retval   地址 + 功能码 + 命令状态 + 校验字节
  */
void Emm_V5_Modify_Ctrl_Mode(uint8_t addr, bool svF, uint8_t ctrl_mode)
{
    uint8_t cmd[16] = {0};

    // 装载命令
    cmd[0] =  addr;                       // 地址
    cmd[1] =  0x46;                       // 功能码
    cmd[2] =  0x69;                       // 辅助码
    cmd[3] =  svF;                        // 是否存储标志，false为不存储，true为存储
    cmd[4] =  ctrl_mode;                  // 控制模式（对应屏幕上的P_Pul菜单），0是关闭脉冲输入引脚，1是开环模式，2是闭环模式，3是让En端口复用为多圈限位开关输入引脚，Dir端口复用为到位输出高电平功能
    cmd[5] =  0x6B;                       // 校验字节

    // 发送命令
    usart_SendCmd(cmd, 6);
}

/**
  * @brief    使能信号控制
  * @param    addr  ：电机地址
  * @param    state ：使能状态     ，true为使能电机，false为关闭电机
  * @param    snF   ：多机同步标志 ，false为不启用，true为启用
  * @retval   地址 + 功能码 + 命令状态 + 校验字节
  */
void Emm_V5_En_Control(uint8_t addr, bool state, bool snF)
{
    uint8_t cmd[16] = {0};

    // 装载命令
    cmd[0] =  addr;                       // 地址
    cmd[1] =  0xF3;                       // 功能码
    cmd[2] =  0xAB;                       // 辅助码
    cmd[3] =  (uint8_t)state;             // 使能状态
    cmd[4] =  snF;                        // 多机同步运动标志
    cmd[5] =  0x6B;                       // 校验字节

    // 发送命令
    usart_SendCmd(cmd, 6);
}

/**
  * @brief    速度模式
  * @param    addr：电机地址
  * @param    dir ：方向       ，0为CW，其余值为CCW
  * @param    vel ：速度       ，范围0 - 5000RPM
  * @param    acc ：加速度     ，范围0 - 255，注意：0是直接启动
  * @param    snF ：多机同步标志，false为不启用，true为启用
  * @retval   地址 + 功能码 + 命令状态 + 校验字节
  */
void Emm_V5_Vel_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, bool snF)
{
    uint8_t cmd[16] = {0};

    // 装载命令
    cmd[0] =  addr;                       // 地址
    cmd[1] =  0xF6;                       // 功能码
    cmd[2] =  dir;                        // 方向
    cmd[3] =  (uint8_t)(vel >> 8)& 0xFF;        // 速度(RPM)高8位字节
    cmd[4] =  (uint8_t)(vel & 0xFF);        // 速度(RPM)低8位字节
    cmd[5] =  acc;                        // 加速度，注意：0是直接启动
    cmd[6] =  snF;                        // 多机同步运动标志
    cmd[7] =  0x6B;                       // 校验字节


    // 发送命令
    usart_SendCmd(cmd, 8);
    delay_ms(5);
}

/**
  * @brief    位置模式
  * @param    addr：电机地址
  * @param    dir ：方向        ，0为CW，其余值为CCW
  * @param    vel ：速度(RPM)   ，范围0 - 5000RPM
  * @param    acc ：加速度      ，范围0 - 255，注意：0是直接启动
  * @param    clk ：脉冲数      ，范围0- (2^32 - 1)个
  * @param    raF ：相位/绝对标志，false为相对运动，true为绝对值运动
  * @param    snF ：多机同步标志 ，false为不启用，true为启用
  * @retval   地址 + 功能码 + 命令状态 + 校验字节
  */
void Emm_V5_Pos_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, uint32_t clk, bool raF, bool snF)
{
    uint8_t cmd[16] = {0};

    // 装载命令
    cmd[0]  =  addr;                      // 地址
    cmd[1]  =  0xFD;                      // 功能码
    cmd[2]  =  dir;                       // 方向
    cmd[3]  =  (uint8_t)(vel >> 8);       // 速度(RPM)高8位字节
    cmd[4]  =  (uint8_t)(vel >> 0);       // 速度(RPM)低8位字节
    cmd[5]  =  acc;                       // 加速度，注意：0是直接启动
    cmd[6]  =  (uint8_t)(clk >> 24);      // 脉冲数(bit24 - bit31)
    cmd[7]  =  (uint8_t)(clk >> 16);      // 脉冲数(bit16 - bit23)
    cmd[8]  =  (uint8_t)(clk >> 8);       // 脉冲数(bit8  - bit15)
    cmd[9]  =  (uint8_t)(clk >> 0);       // 脉冲数(bit0  - bit7 )
    cmd[10] =  raF;                       // 相位/绝对标志，false为相对运动，true为绝对值运动
    cmd[11] =  snF;                       // 多机同步运动标志，false为不启用，true为启用
    cmd[12] =  0x6B;                      // 校验字节

    // 发送命令
    usart_SendCmd(cmd, 13);
    delay_ms(5);

}

/**
  * @brief    立即停止（所有控制模式都通用）
  * @param    addr  ：电机地址
  * @param    snF   ：多机同步标志，false为不启用，true为启用
  * @retval   地址 + 功能码 + 命令状态 + 校验字节
  */
void Emm_V5_Stop_Now(uint8_t addr, bool snF)
{
    uint8_t cmd[16] = {0};

    // 装载命令
    cmd[0] =  addr;                       // 地址
    cmd[1] =  0xFE;                       // 功能码
    cmd[2] =  0x98;                       // 辅助码
    cmd[3] =  snF;                        // 多机同步运动标志
    cmd[4] =  0x6B;                       // 校验字节

    // 发送命令
    usart_SendCmd(cmd, 5);
    delay_ms(10);

}

/**
  * @brief    多机同步运动
  * @param    addr  ：电机地址
  * @retval   地址 + 功能码 + 命令状态 + 校验字节
  */
void Emm_V5_Synchronous_motion(uint8_t addr)
{
    uint8_t cmd[16] = {0};

    // 装载命令
    cmd[0] =  addr;                       // 地址
    cmd[1] =  0xFF;                       // 功能码
    cmd[2] =  0x66;                       // 辅助码
    cmd[3] =  0x6B;                       // 校验字节

    // 发送命令
    usart_SendCmd(cmd, 4);
    delay_ms(10);

}

/**
  * @brief    设置单圈回零的零点位置
  * @param    addr  ：电机地址
  * @param    svF   ：是否存储标志，false为不存储，true为存储
  * @retval   地址 + 功能码 + 命令状态 + 校验字节
  */
void Emm_V5_Origin_Set_O(uint8_t addr, bool svF)
{
    uint8_t cmd[16] = {0};

    // 装载命令
    cmd[0] =  addr;                       // 地址
    cmd[1] =  0x93;                       // 功能码
    cmd[2] =  0x88;                       // 辅助码
    cmd[3] =  svF;                        // 是否存储标志，false为不存储，true为存储
    cmd[4] =  0x6B;                       // 校验字节

    // 发送命令
    usart_SendCmd(cmd, 5);
}

/**
  * @brief    修改回零参数
  * @param    addr  ：电机地址
  * @param    svF   ：是否存储标志，false为不存储，true为存储
  * @param    o_mode ：回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零
  * @param    o_dir  ：回零方向，0为CW，其余值为CCW
  * @param    o_vel  ：回零速度，单位：RPM（转/分钟）
  * @param    o_tm   ：回零超时时间，单位：毫秒
  * @param    sl_vel ：无限位碰撞回零检测转速，单位：RPM（转/分钟）
  * @param    sl_ma  ：无限位碰撞回零检测电流，单位：Ma（毫安）
  * @param    sl_ms  ：无限位碰撞回零检测时间，单位：Ms（毫秒）
  * @param    potF   ：上电自动触发回零，false为不使能，true为使能
  * @retval   地址 + 功能码 + 命令状态 + 校验字节
  */
void Emm_V5_Origin_Modify_Params(uint8_t addr, bool svF, uint8_t o_mode, uint8_t o_dir, uint16_t o_vel, uint32_t o_tm, uint16_t sl_vel, uint16_t sl_ma, uint16_t sl_ms, bool potF)
{
    uint8_t cmd[32] = {0};

    // 装载命令
    cmd[0] =  addr;                       // 地址
    cmd[1] =  0x4C;                       // 功能码
    cmd[2] =  0xAE;                       // 辅助码
    cmd[3] =  svF;                        // 是否存储标志，false为不存储，true为存储
    cmd[4] =  o_mode;                     // 回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零
    cmd[5] =  o_dir;                      // 回零方向
    cmd[6]  =  (uint8_t)(o_vel >> 8);     // 回零速度(RPM)高8位字节
    cmd[7]  =  (uint8_t)(o_vel >> 0);     // 回零速度(RPM)低8位字节
    cmd[8]  =  (uint8_t)(o_tm >> 24);     // 回零超时时间(bit24 - bit31)
    cmd[9]  =  (uint8_t)(o_tm >> 16);     // 回零超时时间(bit16 - bit23)
    cmd[10] =  (uint8_t)(o_tm >> 8);      // 回零超时时间(bit8  - bit15)
    cmd[11] =  (uint8_t)(o_tm >> 0);      // 回零超时时间(bit0  - bit7 )
    cmd[12] =  (uint8_t)(sl_vel >> 8);    // 无限位碰撞回零检测转速(RPM)高8位字节
    cmd[13] =  (uint8_t)(sl_vel >> 0);    // 无限位碰撞回零检测转速(RPM)低8位字节
    cmd[14] =  (uint8_t)(sl_ma >> 8);     // 无限位碰撞回零检测电流(Ma)高8位字节
    cmd[15] =  (uint8_t)(sl_ma >> 0);     // 无限位碰撞回零检测电流(Ma)低8位字节
    cmd[16] =  (uint8_t)(sl_ms >> 8);     // 无限位碰撞回零检测时间(Ms)高8位字节
    cmd[17] =  (uint8_t)(sl_ms >> 0);     // 无限位碰撞回零检测时间(Ms)低8位字节
    cmd[18] =  potF;                      // 上电自动触发回零，false为不使能，true为使能
    cmd[19] =  0x6B;                      // 校验字节

    // 发送命令
    usart_SendCmd(cmd, 20);
}

/**
  * @brief    触发回零
  * @param    addr   ：电机地址
  * @param    o_mode ：回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零
  * @param    snF   ：多机同步标志，false为不启用，true为启用
  * @retval   地址 + 功能码 + 命令状态 + 校验字节
  */
void Emm_V5_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF)
{
    uint8_t cmd[16] = {0};

    // 装载命令
    cmd[0] =  addr;                       // 地址
    cmd[1] =  0x9A;                       // 功能码
    cmd[2] =  o_mode;                     // 回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零
    cmd[3] =  snF;                        // 多机同步运动标志，false为不启用，true为启用
    cmd[4] =  0x6B;                       // 校验字节

    // 发送命令
    usart_SendCmd(cmd, 5);
}

/**
  * @brief    强制中断并退出回零
  * @param    addr  ：电机地址
  * @retval   地址 + 功能码 + 命令状态 + 校验字节
  */
void Emm_V5_Origin_Interrupt(uint8_t addr)
{
    uint8_t cmd[16] = {0};

    // 装载命令
    cmd[0] =  addr;                       // 地址
    cmd[1] =  0x9C;                       // 功能码
    cmd[2] =  0x48;                       // 辅助码
    cmd[3] =  0x6B;                       // 校验字节

    // 发送命令
    usart_SendCmd(cmd, 4);
}
/**
*@brief 单个底盘电机控制函数
*@param _motor 电机对象
*@return 无
*/
void chassis_motor_control_speed(steeping_motor _motor)
{
    char ctrl_speed_arr[8] = {	_motor.ID,
                                  0xF6,
                                  _motor.direction,
                                  _motor.speed >> 8,
                                  _motor.speed,
                                  _motor.acc_speed,
                                  _motor.synchronous,
                                  0x6B};

    usart_SendCmd(ctrl_speed_arr, 8);


    delay_ms(1);
}

/**原地旋转
 * **/
void Emm_V5_Control_dir(float w)
{


        if (w > 0) {
            Emm_V5_Vel_Control(3, 0, w, 0, 0);
            Emm_V5_Vel_Control(4, 0, w, 0, 0);

        } else if (w < 0) {
            Emm_V5_Vel_Control(3, 1, -w, 0, 0);
            Emm_V5_Vel_Control(4, 1, -w, 0, 0);
        } else {
            Emm_V5_Stop_Now(0, 1);
            return;
        }
}
/**运动控制
 * **/
void Emm_V5_Control_dir_speed(float w,float base_speed)
{

    // 定义最大速度（可根据实际情况调整）
    const float MAX_SPEED = 200.0f;

    // 检查输入速度是否在合理范围内
    if (fabs(w) > MAX_SPEED) {
        w = (w > 0) ? MAX_SPEED : -MAX_SPEED;
    }

    if (fabs(base_speed) > MAX_SPEED) {
        base_speed = (base_speed > 0) ? MAX_SPEED : -MAX_SPEED;
    }

    float left_speed, right_speed;

    if (w > 0) {
        // 顺时针旋转
        left_speed = base_speed + w;  // 左电机速度 = 基础速度 + 角速度
        right_speed = base_speed - w; // 右电机速度 = 基础速度 - 角速度
    } else if (w < 0) {
        // 逆时针旋转
        left_speed = base_speed - (-w); // 左电机速度 = 基础速度 - 角速度
        right_speed = base_speed + (-w); // 右电机速度 = 基础速度 + 角速度
    } else {
        // 无旋转，仅基础速度
        left_speed = base_speed;
        right_speed = base_speed;
    }

    // 设置电机速度
    Emm_V5_Vel_Control(3, (left_speed >= 0) ? 0 : 1, fabs(left_speed), 200, 0);
    Emm_V5_Vel_Control(4, (right_speed >= 0) ? 1 : 0, fabs(right_speed), 200, 0);
}

void Emm_speed_circle(float R,float v)
{
    // 假设目标圆周半径为 R，车轮间距为 L，目标速度为 v


    // 计算左右轮速度
    float v_l = (R - L / 2) / R * v; // 左轮速度
    float v_r = (R + L / 2) / R * v; // 右轮速度

    // 设置左轮速度和方向
    int dir_l = (v_l >= 0) ? 1 : -1; // 判断左轮方向
    Emm_V5_Vel_Control(3, dir_l,fabs(v_l),0,1); // 设置左轮速度和方向
    // 设置右轮速度和方向
    int dir_r = (v_r >= 0) ? 1 : -1; // 判断右轮方向
    Emm_V5_Vel_Control(4,  dir_r,fabs(v_r),0,1); // 设置右轮速度和方向

    Emm_V5_Synchronous_motion(0);
}
// float Emm_read_pos(uint8_t addr)
// {
//     Emm_V5_Read_Sys_Params(addr, S_CPOS);
//     while(rxFrameFlag == false); rxFrameFlag = false;
//     if(rxCmd[0] == addr && rxCmd[1] == 0x36 && rxCount == 8)
//     {
//         // 拼接成uint32_t类型
//         pos = (uint32_t)(
//                 ((uint32_t)rxCmd[3] << 24)    |
//                 ((uint32_t)rxCmd[4] << 16)    |
//                 ((uint32_t)rxCmd[5] << 8)     |
//                 ((uint32_t)rxCmd[6] << 0)
//         );

//         // 转换成角度
//         Motor_Cur_Pos = (float)pos * 360.0f / 65536.0f;
//         // 符号
//         //if(rxCmd[2]) { Motor_Cur_Pos = -Motor_Cur_Pos; }
//     }
//     return Motor_Cur_Pos;
// }
// float Emm_read_speed(uint8_t addr)
// {
//     Emm_V5_Read_Sys_Params(addr, S_VEL);
//     while(rxFrameFlag == false); rxFrameFlag = false;
//     if(rxCmd[0] == addr && rxCmd[1] == 0x35 && rxCount == 6)
//     {
//         // 拼接成uint16_t类型数据
//         vel = (uint16_t)(
//                 ((uint16_t)rxCmd[3] << 8)   |
//                 ((uint16_t)rxCmd[4] << 0)
//         );

//         // 实时速度mm/s
//         Motor_Vel = (vel*M_PI*LR)/60;

//         // 符号
// //        if(rxCmd[2]) { Motor_Vel = -Motor_Vel; }
//     }
//     return Motor_Vel;
// }